Solved Problem

In the Stick yaw controller the update yaw correction logic is flawed. If unaided yaw is nan it skips the yaw correction by setting the yaw correction to 0. This works if the unaided yaw is always nan or is never nan, but if it becomes nan during execution for some reason this leads to a large jump in yaw set point during yaw lock.

This is because yaw_correction_ is set to 0 and the yaw setpoint output by the controller (if in yaw lock mode) is:

wrap_pi(yaw_setpoint - yaw_correction_prev + _yaw_correction) = wrap_pi(yaw_setpoint - yaw_correction_prev)

if yaw_correction_ is set to 0 and yaw_correction_prev is non zero a large jump in yaw may occour.

Fixes #{Github issue ID}

Solution

• removes zeroing out of yaw_correction_
  • thereby leaving yaw_correction_prev = yaw_correction_

Changelog Entry

For release notes:

Bugfix: fixed potential yaw instability in MPC flight modes